// OpenCvAdapter.cpp : Defines the entry point for the DLL application.
//

#include "stdafx.h"
#include "OpenCvAdapter.h"


#ifdef _MANAGED
#pragma managed(push, off)
#endif

BOOL APIENTRY DllMain( HMODULE hModule,
                       DWORD  ul_reason_for_call,
                       LPVOID lpReserved
					 )
{
	switch (ul_reason_for_call)
	{
	case DLL_PROCESS_ATTACH:
	case DLL_THREAD_ATTACH:
	case DLL_THREAD_DETACH:
	case DLL_PROCESS_DETACH:
		break;
	}
    return TRUE;
}

#ifdef _MANAGED
#pragma managed(pop)
#endif


bool CapturingStarted;
bool capturingStopped;
bool callibrationStarted;
bool callibrationStopped;

CallibrationData * loadedCallibrationData = 0;
CallibrationData * localCallibrationData = 0;
IplImage * imageSentForCallibration = 0;
StethLocationData* previousLocationData;

//image processing
void MarkKeyPoints(IplImage* sourceImage);
MarkerPoint GetCenterMarkerPoint(IplImage* sourceImage, 
								 IplImage* centerMarkerTemplate, IplImage* leftMarkerTemplate, IplImage* rightMarkerTemplate);

MarkerPoint GetSecondMarkerPoint(IplImage * source, 
							 IplImage * centerMarkerTemplate, IplImage * leftMarkerTemplate, IplImage * rightMarkerTemplate, 
							 IplImage * stethMarkerTemplate, 
							 CvPoint centerMarkerLocation);

MarkerPoint GetSecondMarkerPoint(IplImage * source,
							 IplImage * leftMarkerTemplate, IplImage * rightMarkerTemplate, 
							 IplImage * stethMarkerTemplate);

MarkerPoint GetStethMarkerPoint(IplImage * sourceImage, 
								IplImage * centerMarkerTemplate, IplImage * leftMarkerTemplate, IplImage * rightMarkerTemplate,
								IplImage * stethMarkerTemplate);

void SaveImageRegion(IplImage* sourceImage, CvRect rect, char * fileName);

void UpdateSideMarkers(CvPoint centerMarkerLocation, 
					   CvPoint & leftMarkerLocation, CvPoint & rightMarkerLocation, 
					   CvPoint foundMarkerLocation);

void UpdateCenterMarker(CvPoint firstSecondMarker, CvPoint secondSecondMarker, 
						CvPoint & centerMarkerPoint, CvPoint & leftMarkerPoint, CvPoint & rightMarkerPoint);

void UpdateLocalCallibrationMap(CvPoint centerMarkerPoint, 
								CvPoint leftMarkerPoint,
								CvPoint rightMarkerPoint);

void DrawLocalCallibrationInformation(IplImage * src);
StethLocationData* GetMapLocationData(CvPoint stethLocalLocation);

OPENCVADAPTER_API void StartProcessing()
{
	if (CapturingStarted)
	{
		return;
	}

	CapturingStarted = true;
	capturingStopped = false;

	CvCapture * capture = cvCaptureFromCAM(0);
	IplImage * source = 0;

	//intermediate images for processing 
	IplImage * grayScaleImage = 0;
	while (CapturingStarted)
	{
		if (grayScaleImage != 0)
		{
			cvReleaseImage(&grayScaleImage);
		}
		source = cvQueryFrame(capture);

		//process with some intermediate images
		int width = source->width;
		int height = source->height;
		grayScaleImage = cvCreateImage(cvGetSize(source), IPL_DEPTH_8U, 1);
		cvCvtColor(source, grayScaleImage, CV_RGB2GRAY);


		cvNamedWindow("win1", 1);
		cvShowImage("win1", source);

		SupportiveFunctions::ApplySobel(source, source);

		cvNamedWindow("win", 1);
		cvShowImage("win", source);
		cvWaitKey(1);
	}
	cvDestroyAllWindows();
	cvReleaseCapture(&capture);
	capturingStopped = true;
}

OPENCVADAPTER_API void StartCapturing(long sourceImagePushedDelegateAddress, 
									  long StethLocationDataPushedDelegateAddress)
{
	if (CapturingStarted)
	{
		return;
	}

	CapturingStarted = true;
	capturingStopped = false;
	delete loadedCallibrationData;
	loadedCallibrationData = 0;
	loadedCallibrationData = CallibrationData::Deserialize();
	localCallibrationData = new CallibrationData();
	localCallibrationData->Copy(loadedCallibrationData);

	ImagePushedDelegate sourceImagePushedDelegate = (ImagePushedDelegate)sourceImagePushedDelegateAddress;
	StethLocationDataPushedDelegate stethLocationDataPushedDelegate = (StethLocationDataPushedDelegate)StethLocationDataPushedDelegateAddress;

	CvCapture * capture = cvCaptureFromCAM(0);
	IplImage * source = 0;

	//recored
#ifdef RECORD
	double fps = 30;
	CvSize size = cvSize((int)cvGetCaptureProperty( capture, CV_CAP_PROP_FRAME_WIDTH),
		(int)cvGetCaptureProperty( capture, CV_CAP_PROP_FRAME_HEIGHT));
	CvVideoWriter *writer = cvCreateVideoWriter("recorded.avi", CV_FOURCC('M','J','P','G'), fps, size, 0);
#endif


	//intermediate images for processing 
	IplImage * grayScaleImage = 0;
	while (CapturingStarted)
	{
		if (grayScaleImage != 0)
		{
			cvReleaseImage(&grayScaleImage);
		}
		source = cvQueryFrame(capture);

		//process with some intermediate images
		int width = source->width;
		int height = source->height;
		grayScaleImage = cvCreateImage(cvGetSize(source), IPL_DEPTH_8U, 1);
		cvCvtColor(source, grayScaleImage, CV_RGB2GRAY);

		//record
#ifdef RECORD
		 cvWriteFrame( writer, grayScaleImage );
#endif

		//processing
		MarkKeyPoints(grayScaleImage);

		uchar* imageData = (uchar*)grayScaleImage->imageData;
		sourceImagePushedDelegate(width, height, imageData);
		if (previousLocationData != 0)
		{
			stethLocationDataPushedDelegate(previousLocationData);
		}
	}

#ifdef RECORD
	cvReleaseVideoWriter(&writer);
#endif
	cvReleaseCapture(&capture);
	capturingStopped = true;
}

OPENCVADAPTER_API void StartCapturingFromFile(const char* fileName,
											  long sourceImagePushedDelegateAddress, 
											  long StethLocationDataPushedDelegateAddress,
											  long CapturingFinishedDelegateAddress)
{
	if (CapturingStarted)
	{
		return;
	}

	CapturingStarted = true;
	capturingStopped = false;
	delete loadedCallibrationData;
	loadedCallibrationData = 0;
	loadedCallibrationData = CallibrationData::Deserialize();
	localCallibrationData = new CallibrationData();
	localCallibrationData->Copy(loadedCallibrationData);

	ImagePushedDelegate sourceImagePushedDelegate = (ImagePushedDelegate)sourceImagePushedDelegateAddress;
	StethLocationDataPushedDelegate stethLocationDataPushedDelegate = (StethLocationDataPushedDelegate)StethLocationDataPushedDelegateAddress;
	CapturingFinishedDelegate capturingFinishedDelegate = (CapturingFinishedDelegate)CapturingFinishedDelegateAddress;

	CvCapture * capture = cvCaptureFromFile(fileName);
	IplImage * source = 0;

	//intermediate images for processing 
	IplImage * grayScaleImage = 0;
	while ((source = cvQueryFrame(capture)) && CapturingStarted)
	{
		if (grayScaleImage != 0)
		{
			cvReleaseImage(&grayScaleImage);
		}
		//process with some intermediate images
		int width = source->width;
		int height = source->height;
		grayScaleImage = cvCreateImage(cvGetSize(source), IPL_DEPTH_8U, 1);
		cvCvtColor(source, grayScaleImage, CV_RGB2GRAY);

		//processing
		MarkKeyPoints(grayScaleImage);

		uchar* imageData = (uchar*)grayScaleImage->imageData;
		sourceImagePushedDelegate(width, height, imageData);
		if (previousLocationData != 0)
		{
			stethLocationDataPushedDelegate(previousLocationData);
		}
	}
	cvReleaseCapture(&capture);
	delete loadedCallibrationData;
	loadedCallibrationData = 0;
	delete localCallibrationData;
	localCallibrationData = 0;
	delete previousLocationData;
	previousLocationData = 0;

	capturingStopped = true;
	CapturingStarted = false;
	capturingFinishedDelegate();
}

OPENCVADAPTER_API void StartCaptureAndRecord(const char* fileName, 
											 long sourceImagePushedDelegateAddress, 
											 long StethLocationDataPushedDelegateAddress)
{
	if (CapturingStarted)
	{
		return;
	}

	CapturingStarted = true;
	capturingStopped = false;
	delete loadedCallibrationData;
	loadedCallibrationData = 0;
	loadedCallibrationData = CallibrationData::Deserialize();
	localCallibrationData = new CallibrationData();
	localCallibrationData->Copy(loadedCallibrationData);

	ImagePushedDelegate sourceImagePushedDelegate = (ImagePushedDelegate)sourceImagePushedDelegateAddress;
	StethLocationDataPushedDelegate stethLocationDataPushedDelegate = (StethLocationDataPushedDelegate)StethLocationDataPushedDelegateAddress;

	CvCapture * capture = cvCaptureFromCAM(0);
	IplImage * source = 0;

	//recored
	double fps = 30;
	CvSize size = cvSize((int)cvGetCaptureProperty( capture, CV_CAP_PROP_FRAME_WIDTH),
		(int)cvGetCaptureProperty( capture, CV_CAP_PROP_FRAME_HEIGHT));
	CvVideoWriter *writer = cvCreateVideoWriter(fileName, CV_FOURCC('M','J','P','G'), fps, size, 0);


	//intermediate images for processing 
	IplImage * grayScaleImage = 0;
	while (CapturingStarted)
	{
		if (grayScaleImage != 0)
		{
			cvReleaseImage(&grayScaleImage);
		}
		source = cvQueryFrame(capture);

		//process with some intermediate images
		int width = source->width;
		int height = source->height;
		grayScaleImage = cvCreateImage(cvGetSize(source), IPL_DEPTH_8U, 1);
		cvCvtColor(source, grayScaleImage, CV_RGB2GRAY);

		//record
		 cvWriteFrame( writer, grayScaleImage );

		//processing
		MarkKeyPoints(grayScaleImage);

		uchar* imageData = (uchar*)grayScaleImage->imageData;
		sourceImagePushedDelegate(width, height, imageData);
		if (previousLocationData != 0)
		{
			stethLocationDataPushedDelegate(previousLocationData);
		}
	}

	cvReleaseVideoWriter(&writer);
	cvReleaseCapture(&capture);
	capturingStopped = true;
}

OPENCVADAPTER_API void StopCapturing()
{	
	if (CapturingStarted)
	{
		CapturingStarted = false;
		while (!capturingStopped)
		{
			Sleep(200);
		}
	}
	delete loadedCallibrationData;
	loadedCallibrationData = 0;
	delete localCallibrationData;
	localCallibrationData = 0;
	delete previousLocationData;
	previousLocationData = 0;
}


OPENCVADAPTER_API void StartCallibration(long sourceImagePushedDelegateAddress)
{
	callibrationStarted = true;
	callibrationStopped = false;
	ImagePushedDelegate sourceImagePushedDelegate = (ImagePushedDelegate)sourceImagePushedDelegateAddress;

	CvCapture * capture = cvCaptureFromCAM(0);
	IplImage * source = 0;
	//intermediate images for processing 
	while (callibrationStarted)
	{
		if (imageSentForCallibration != 0)
		{
			cvReleaseImage(&imageSentForCallibration);
		}
		source = cvQueryFrame(capture);

		//process with some intermediate images
		int width = source->width;
		int height = source->height;
		imageSentForCallibration = cvCreateImage(cvGetSize(source), IPL_DEPTH_8U, 1);
		cvCvtColor(source, imageSentForCallibration, CV_RGB2GRAY);

		//push image for callibration
		uchar* imageData = (uchar*)imageSentForCallibration->imageData;
		sourceImagePushedDelegate(width, height, imageData);
	}
	callibrationStopped = true;
	cvReleaseCapture(&capture);
	cvDestroyAllWindows();
}

OPENCVADAPTER_API void StopCallibration()
{
	callibrationStarted = false;
}

OPENCVADAPTER_API void UpdateDataAndStopCallibration(BYTE* callibrationInfo, int length)
{
	callibrationStarted = false;
	while (!callibrationStopped)
	{
		Sleep(200);
	}

	delete localCallibrationData;
	loadedCallibrationData = 0;
	loadedCallibrationData = CallibrationData::ReadAndSaveCallibrationData(imageSentForCallibration, callibrationInfo);
}


void MarkKeyPoints(IplImage* sourceImage)
{
	IplImage* centerMarkerTemplate = cvLoadImage(SupportiveFunctions::CenterMarkerFile, 0);
	IplImage* leftMarkerTemplate = cvLoadImage(SupportiveFunctions::LeftMarkerFile, 0);
	IplImage* rightMarkerTemplate = cvLoadImage(SupportiveFunctions::RightMarkerFile, 0);
	IplImage* stethMarkerTemplate = cvLoadImage(SupportiveFunctions::StethMarkerFile, 0);

	bool allMarkersDetected = false;
	MarkerPoint centerMarkerLocation = GetCenterMarkerPoint(sourceImage, 
		centerMarkerTemplate, 
		leftMarkerTemplate,
		rightMarkerTemplate);
	if (centerMarkerLocation.GetSuccessScore() > 0)
	{
		//center marker point successfully detected
		cvCircle(sourceImage, centerMarkerLocation.GetLocation(), 10, cvScalar(255, 0, 0, 0));
		//remake a relevant markers
		MarkerPoint secondMarkerLocation = GetSecondMarkerPoint(sourceImage, 
							 centerMarkerTemplate, 
							 leftMarkerTemplate,
							 rightMarkerTemplate, 
							 stethMarkerTemplate,
							 centerMarkerLocation.GetLocation());
		if (secondMarkerLocation.GetSuccessScore() > 0)
		{
			//second marker successfully deteced
			cvCircle(sourceImage, secondMarkerLocation.GetLocation(), 10, cvScalar(255, 255, 0, 0));
			//recalc parameters
			CvPoint leftMarkerPoint = cvPoint(0,0);
			CvPoint rightMarkerPoint = cvPoint(0,0);
			UpdateSideMarkers(centerMarkerLocation.GetLocation(), leftMarkerPoint, rightMarkerPoint, secondMarkerLocation.GetLocation()); 
			UpdateLocalCallibrationMap(centerMarkerLocation.GetLocation(), leftMarkerPoint, rightMarkerPoint);
			//draw callibration info
			DrawLocalCallibrationInformation(sourceImage);
			allMarkersDetected = true;
		}
		else
		{
			//second marker not deteced - failure
			cvCircle(sourceImage, secondMarkerLocation.GetLocation(), 10, cvScalar(0, 0, 0, 0));
			//do not update parameters
		}
	}
	else
	{
		//center marker point not detected
		//the point which is detected must be a second marker otherwise failure
		MarkerPoint firstSecondMarkerLocation = GetSecondMarkerPoint(sourceImage,
			leftMarkerTemplate,
			rightMarkerTemplate,
			stethMarkerTemplate);
		if (firstSecondMarkerLocation.GetSuccessScore() > 0)
		{
			//close second marker and find either center marker or another second marker
			CvRect centerMarkerRegion =  cvRect(firstSecondMarkerLocation.GetLocation().x - leftMarkerTemplate->width / 2,
			firstSecondMarkerLocation.GetLocation().y - leftMarkerTemplate->height / 2,
			leftMarkerTemplate->width,
			leftMarkerTemplate->height);
			IplImage* sourceWithNoSecondMarker = SupportiveFunctions::SetZeroToRegion(sourceImage, centerMarkerRegion);
			//find center marker
			MarkerPoint centerMarkerLocation = GetCenterMarkerPoint(sourceWithNoSecondMarker, 
				centerMarkerTemplate, 
				leftMarkerTemplate,
				rightMarkerTemplate);
			if (centerMarkerLocation.GetSuccessScore() > 0)
			{
				//center marker found
				//recalc parameters
				CvPoint leftMarkerPoint = cvPoint(0,0);
				CvPoint rightMarkerPoint = cvPoint(0,0);
				UpdateSideMarkers(centerMarkerLocation.GetLocation(), leftMarkerPoint, rightMarkerPoint, firstSecondMarkerLocation.GetLocation()); 
				UpdateLocalCallibrationMap(centerMarkerLocation.GetLocation(), leftMarkerPoint, rightMarkerPoint);
				//draw callibration info
				DrawLocalCallibrationInformation(sourceImage);
				allMarkersDetected = true;
			}
			else
			{
				//stil no sign of center marker, find another second marker
				MarkerPoint secondSecondMarkerLocation = GetSecondMarkerPoint(sourceWithNoSecondMarker,
					leftMarkerTemplate,
					rightMarkerTemplate,
					stethMarkerTemplate);
				if (secondSecondMarkerLocation.GetSuccessScore() > 0)
				{
					CvPoint centerMarkerPoint = cvPoint(0,0);
					CvPoint leftMarkerPoint = cvPoint(0,0);
					CvPoint rightMarkerPoint = cvPoint(0,0);
					UpdateCenterMarker(firstSecondMarkerLocation.GetLocation(), secondSecondMarkerLocation.GetLocation(),
						centerMarkerPoint, leftMarkerPoint, rightMarkerPoint);
					UpdateLocalCallibrationMap(centerMarkerPoint, leftMarkerPoint, rightMarkerPoint);

					DrawLocalCallibrationInformation(sourceImage);
					allMarkersDetected = true;
				}
				else
				{
					//failure: 
					//do not update parameters
				}
			}

		}
		else
		{
			//second marker not deteced - failure
			cvCircle(sourceImage, firstSecondMarkerLocation.GetLocation(), 10, cvScalar(0, 0, 0, 0));
		}
	}

	if (allMarkersDetected)
	{
		MarkerPoint stethMarkerPoint = GetStethMarkerPoint(sourceImage, 
		centerMarkerTemplate, leftMarkerTemplate, rightMarkerTemplate,
		stethMarkerTemplate);
		if (stethMarkerPoint.GetSuccessScore() > 60)
		{
			//steth found
			cvCircle(sourceImage, stethMarkerPoint.GetLocation(), 10, cvScalar(0, 0, 255, 0));
			//update previous location data
			delete previousLocationData;
			previousLocationData = 0;
			previousLocationData = GetMapLocationData(stethMarkerPoint.GetLocation());
		}
		else
		{
			//steth missing, steth might be actually not in the scene. or might have been occluded
			//this is not an error.
		}
	}

	cvReleaseImage(&centerMarkerTemplate);
	cvReleaseImage(&leftMarkerTemplate);
	cvReleaseImage(&rightMarkerTemplate);
	cvReleaseImage(&stethMarkerTemplate);
}

MarkerPoint GetCenterMarkerPoint(IplImage* sourceImage, 
								 IplImage* centerMarkerTemplate,
								 IplImage* leftMarkerTemplate, 
								 IplImage* rightMarkerTemplate)
{
	int centerMarkerValue = 0;
	CvPoint centerMatchedPoint = SupportiveFunctions::GetMatchWithMarker(sourceImage, centerMarkerTemplate, centerMarkerValue);
	MarkerPoint markerPoint;
	if (SupportiveFunctions::WithinRegion(sourceImage, centerMarkerTemplate, centerMatchedPoint))
	{
		markerPoint.SetMarkerPoint(centerMatchedPoint);
		//crop center marker matched area
		CvRect rect = cvRect(centerMatchedPoint.x - centerMarkerTemplate->width / 2,
			centerMatchedPoint.y - centerMarkerTemplate->height / 2,
			centerMarkerTemplate->width,
			centerMarkerTemplate->height);
		IplImage * foundCenterMarkerImage =  SupportiveFunctions::GetCroppedImage(sourceImage, rect);
		
		//get maximum corelation with other markers of this center marker matched area
		bool withinOthersRegions;
		int minValWithOtherMarkers;
		CvPoint leftMarkerMatchedPoint = SupportiveFunctions::GetMatchWithMarker(foundCenterMarkerImage, leftMarkerTemplate, minValWithOtherMarkers);
		bool leftMarkerWithinRegion = SupportiveFunctions::WithinRegion(foundCenterMarkerImage, leftMarkerTemplate, leftMarkerMatchedPoint);
		CvPoint rightMarkerMatchedPoint = SupportiveFunctions::GetMatchWithMarker(foundCenterMarkerImage, rightMarkerTemplate, minValWithOtherMarkers);
		bool rightMarkerWithinRegion = SupportiveFunctions::WithinRegion(foundCenterMarkerImage, rightMarkerTemplate, rightMarkerMatchedPoint);

		//get difference with other markers 
		int diffSourceAndCenterTemplate = SupportiveFunctions::GetDifference(foundCenterMarkerImage, centerMarkerTemplate, cvPoint(centerMarkerTemplate->width / 2, centerMarkerTemplate->height / 2));
		int diffSourceAndLeftTemplate = foundCenterMarkerImage->width * foundCenterMarkerImage->height;
		int diffSourceAndRightTemplate = foundCenterMarkerImage->width * foundCenterMarkerImage->height;
		if (leftMarkerWithinRegion)
		{
			diffSourceAndLeftTemplate = SupportiveFunctions::GetDifference(foundCenterMarkerImage, leftMarkerTemplate, leftMarkerMatchedPoint);
		}
		if (rightMarkerWithinRegion)
		{
			diffSourceAndRightTemplate = SupportiveFunctions::GetDifference(foundCenterMarkerImage, rightMarkerTemplate, rightMarkerMatchedPoint);
		}


		if (diffSourceAndCenterTemplate < diffSourceAndLeftTemplate
			&& diffSourceAndCenterTemplate < diffSourceAndRightTemplate)
		{
			markerPoint.SetSuccessScore(100);
		}
		else
		{
			markerPoint.SetSuccessScore(0);
		}
		cvReleaseImage(&foundCenterMarkerImage);	
	}
	else
	{
		markerPoint.SetMarkerPoint(cvPoint(0, 0));
		markerPoint.SetSuccessScore(0);
	}
	return markerPoint;
}






MarkerPoint GetSecondMarkerPoint(IplImage * source, 
							 IplImage * centerMarkerTemplate, 
							 IplImage * leftMarkerTemplate,
							 IplImage * rightMarkerTemplate, 
							 IplImage * stethMarkerTemplate, 
							 CvPoint centerMarkerLocation)
{
	MarkerPoint secondMarkerPoint;

	IplImage * sourceWithNoCenterMarker = 0;
	//hide center marker
	if (centerMarkerTemplate != 0)
	{
		CvRect centerMarkerRegion =  cvRect(centerMarkerLocation.x - centerMarkerTemplate->width / 2,
			centerMarkerLocation.y - centerMarkerTemplate->height / 2,
			centerMarkerTemplate->width,
			centerMarkerTemplate->height);
		sourceWithNoCenterMarker = SupportiveFunctions::SetZeroToRegion(source, centerMarkerRegion);
	}
	else
	{
		sourceWithNoCenterMarker = cvCloneImage(source);
	}
	//get match on right and left markers 
	int minValueWithLeftMarker = 0;
	CvPoint leftMarkerBestMatch = SupportiveFunctions::GetMatchWithMarker(sourceWithNoCenterMarker, leftMarkerTemplate, minValueWithLeftMarker);
	bool leftMarkerWithinRegion = SupportiveFunctions::WithinRegion(sourceWithNoCenterMarker, leftMarkerTemplate, leftMarkerBestMatch);
	int minValueWithRightMarker = 0;
	CvPoint rightMarkerBestMatch = SupportiveFunctions::GetMatchWithMarker(sourceWithNoCenterMarker, rightMarkerTemplate, minValueWithRightMarker);
	bool rightMarkerWithinRegion = SupportiveFunctions::WithinRegion(sourceWithNoCenterMarker, rightMarkerTemplate, rightMarkerBestMatch);

	
	//find the template with best match
	IplImage * foundSecondMarkerImage;
	int diffSourceAndLeftRightTemplate;
	bool canProceed = true;
	if (leftMarkerWithinRegion && (minValueWithLeftMarker <= minValueWithRightMarker))
	{
		//left marker template has provided the best match
		secondMarkerPoint.SetMarkerPoint(leftMarkerBestMatch);
		CvRect rect = cvRect(leftMarkerBestMatch.x - leftMarkerTemplate->width / 2,
			leftMarkerBestMatch.y - leftMarkerTemplate->height / 2,
			leftMarkerTemplate->width,
			leftMarkerTemplate->height);
		foundSecondMarkerImage =  SupportiveFunctions::GetCroppedImage(source, rect);
		diffSourceAndLeftRightTemplate = SupportiveFunctions::GetDifference(foundSecondMarkerImage, 
			leftMarkerTemplate, 
			cvPoint(leftMarkerTemplate->width / 2, leftMarkerTemplate->height / 2));
	}
	else if (rightMarkerWithinRegion && (minValueWithLeftMarker > minValueWithRightMarker))
	{
		//right marker template has provided the best match
		secondMarkerPoint.SetMarkerPoint(rightMarkerBestMatch);
		CvRect rect = cvRect(rightMarkerBestMatch.x - rightMarkerTemplate->width / 2,
			rightMarkerBestMatch.y - rightMarkerTemplate->height / 2,
			rightMarkerTemplate->width,
			rightMarkerTemplate->height);
		foundSecondMarkerImage =  SupportiveFunctions::GetCroppedImage(source, rect);
		diffSourceAndLeftRightTemplate = SupportiveFunctions::GetDifference(foundSecondMarkerImage, 
			rightMarkerTemplate, 
			cvPoint(rightMarkerTemplate->width / 2, rightMarkerTemplate->height / 2));
	}
	else
	{
		//both markers are out of region 
		foundSecondMarkerImage = 0;
		canProceed = false;
	}

	if (canProceed)
	{
		//get differences with other possible cases, template image
		bool withinOthersRegions;
		CvPoint stethMarkerMatchedPoint = SupportiveFunctions::GetMatchWithMarker(foundSecondMarkerImage, stethMarkerTemplate);
		bool stethMarkerWithinRegion = SupportiveFunctions::WithinRegion(foundSecondMarkerImage, stethMarkerTemplate, stethMarkerMatchedPoint);
		int diffSourceAndStethTemplate = foundSecondMarkerImage->width * foundSecondMarkerImage->height;
		if (stethMarkerWithinRegion)
		{
			diffSourceAndStethTemplate = SupportiveFunctions::GetDifference(foundSecondMarkerImage, stethMarkerTemplate, stethMarkerMatchedPoint);
		}

		if (diffSourceAndLeftRightTemplate < diffSourceAndStethTemplate)
		{
			secondMarkerPoint.SetSuccessScore(100);
		}
		else
		{
			secondMarkerPoint.SetSuccessScore(0);
		}
	}
	else
	{
		secondMarkerPoint.SetSuccessScore(0);
	}


	cvReleaseImage(&foundSecondMarkerImage);
	cvReleaseImage(&sourceWithNoCenterMarker);
	return secondMarkerPoint;
}



MarkerPoint GetSecondMarkerPoint(IplImage * source,
							 IplImage * leftMarkerTemplate,
							 IplImage * rightMarkerTemplate, 
							 IplImage * stethMarkerTemplate)
{

	IplImage* centerMarkerTemplateDummy = 0;
	CvPoint centerMarkerLocationDummy = cvPoint(0, 0);
	MarkerPoint secondMarkerPoint = GetSecondMarkerPoint(source, 
		centerMarkerTemplateDummy, 
		leftMarkerTemplate,
		rightMarkerTemplate, 
		stethMarkerTemplate, 
		centerMarkerLocationDummy);
	return secondMarkerPoint;
}

void UpdateSideMarkers(CvPoint centerMarkerPoint, 
					   CvPoint & leftMarkerPoint, CvPoint & rightMarkerPoint, 
					   CvPoint foundMarkerLocation)
{
	if (foundMarkerLocation.x >= centerMarkerPoint.x)
	{
		leftMarkerPoint = foundMarkerLocation;
		rightMarkerPoint = MathUtil::GetTranslatedPoint(loadedCallibrationData->GetCenterMarkerPoint(), 
			loadedCallibrationData->GetLeftMarkerPoint(),
			loadedCallibrationData->GetRightMarkerPoint(),
			centerMarkerPoint,
			leftMarkerPoint);
	}
	else
	{
		rightMarkerPoint = foundMarkerLocation;
		leftMarkerPoint = MathUtil::GetTranslatedPoint(loadedCallibrationData->GetCenterMarkerPoint(), 
			loadedCallibrationData->GetRightMarkerPoint(),
			loadedCallibrationData->GetLeftMarkerPoint(),
			centerMarkerPoint,
			rightMarkerPoint);
	}
}


void UpdateCenterMarker(CvPoint firstSecondMarker, CvPoint secondSecondMarker, 
						CvPoint & centerMarkerPoint, CvPoint & leftMarkerPoint, CvPoint & rightMarkerPoint)
{
	if (firstSecondMarker.x > secondSecondMarker.x)
	{
		leftMarkerPoint = firstSecondMarker;
		rightMarkerPoint = secondSecondMarker;
	}
	else
	{
		leftMarkerPoint = secondSecondMarker;
		rightMarkerPoint = firstSecondMarker;
	}

	centerMarkerPoint = MathUtil::GetTranslatedPoint(loadedCallibrationData->GetLeftMarkerPoint(),
		loadedCallibrationData->GetRightMarkerPoint(),
		loadedCallibrationData->GetCenterMarkerPoint(), 
		leftMarkerPoint,
		rightMarkerPoint);
}
void UpdateLocalCallibrationMap(CvPoint centerMarkerPoint, 
								CvPoint leftMarkerPoint,
								CvPoint rightMarkerPoint)
{
	localCallibrationData->SetCenterMarkerPoint(centerMarkerPoint.x, centerMarkerPoint.y);
	localCallibrationData->SetLeftMarkerPoint(leftMarkerPoint.x, leftMarkerPoint.y);
	localCallibrationData->SetRightMarkerPoint(rightMarkerPoint.x, rightMarkerPoint.y);

	//calculate other based on center and left marker points
	CvPoint newJugularNotchPoint = MathUtil::GetTranslatedPoint(loadedCallibrationData->GetCenterMarkerPoint(), 
		loadedCallibrationData->GetLeftMarkerPoint(),
		loadedCallibrationData->GetJugularNotchPoint(),
		centerMarkerPoint,
		leftMarkerPoint);
	localCallibrationData->SetJugularNotchPoint(newJugularNotchPoint.x, newJugularNotchPoint.y);
	CvPoint newXiphoidProcessPoint = MathUtil::GetTranslatedPoint(loadedCallibrationData->GetCenterMarkerPoint(), 
		loadedCallibrationData->GetLeftMarkerPoint(),
		loadedCallibrationData->GetXiphoidProcessPoint(),
		centerMarkerPoint,
		leftMarkerPoint);
	localCallibrationData->SetXiphoidProcessPoint(newXiphoidProcessPoint.x, newXiphoidProcessPoint.y);
	CvPoint newLeftCostalMarginPoint = MathUtil::GetTranslatedPoint(loadedCallibrationData->GetCenterMarkerPoint(), 
		loadedCallibrationData->GetLeftMarkerPoint(),
		loadedCallibrationData->GetLeftCostalMarginPoint(),
		centerMarkerPoint,
		leftMarkerPoint);
	localCallibrationData->SetLeftCostalMarginPoint(newLeftCostalMarginPoint.x, newLeftCostalMarginPoint.y);
	CvPoint newRightCostalMarginPoint = MathUtil::GetTranslatedPoint(loadedCallibrationData->GetCenterMarkerPoint(), 
		loadedCallibrationData->GetLeftMarkerPoint(),
		loadedCallibrationData->GetRightCostalMarginPoint(),
		centerMarkerPoint,
		leftMarkerPoint);
	localCallibrationData->SetRightCostalMarginPoint(newRightCostalMarginPoint.x, newRightCostalMarginPoint.y);
}

void DrawLocalCallibrationInformation(IplImage * src)
{
	cvCircle(src, localCallibrationData->GetCenterMarkerPoint(), 5, cvScalar(255, 255, 255, 0));
	cvCircle(src, localCallibrationData->GetLeftMarkerPoint(), 5, cvScalar(255, 255, 255, 0));
	cvCircle(src, localCallibrationData->GetRightMarkerPoint(), 5, cvScalar(255, 255, 255, 0));

	cvCircle(src, localCallibrationData->GetJugularNotchPoint(), 5, cvScalar(255, 255, 255, 0));
	cvCircle(src, localCallibrationData->GetXiphoidProcessPoint(), 5, cvScalar(255, 255, 255, 0));
	cvCircle(src, localCallibrationData->GetLeftCostalMarginPoint(), 5, cvScalar(255, 255, 255, 0));
	cvCircle(src, localCallibrationData->GetRightCostalMarginPoint(), 5, cvScalar(255, 255, 255, 0));
}

IplImage* HideSupportiveMarkers(IplImage * sourceImage,
						   CvPoint centerMarkerPont, IplImage * centerMarkerTemplate,
						   CvPoint leftMarkerPoint, IplImage * leftMarkerTemplate,
						   CvPoint rightMarkerPoint, IplImage * rightMarkerTemplate)
{
	IplImage* processedImage = cvCloneImage(sourceImage);
	SupportiveFunctions::SetZeroOnRegion(processedImage, 
		cvRect(centerMarkerPont.x - centerMarkerTemplate->width / 2, 
		centerMarkerPont.y - centerMarkerTemplate->height / 2,
		centerMarkerTemplate->width,
		centerMarkerTemplate->height));
	SupportiveFunctions::SetZeroOnRegion(processedImage, 
		cvRect(leftMarkerPoint.x - leftMarkerTemplate->width / 2, 
		leftMarkerPoint.y - leftMarkerTemplate->height / 2,
		leftMarkerTemplate->width,
		leftMarkerTemplate->height));
	SupportiveFunctions::SetZeroOnRegion(processedImage, 
		cvRect(rightMarkerPoint.x - rightMarkerTemplate->width / 2, 
		rightMarkerPoint.y - rightMarkerTemplate->height / 2,
		rightMarkerTemplate->width,
		rightMarkerTemplate->height));

	return processedImage;
}

MarkerPoint GetStethMarkerPoint(IplImage * sourceImage, 
								IplImage * centerMarkerTemplate, IplImage * leftMarkerTemplate, IplImage * rightMarkerTemplate,
								IplImage * stethMarkerTemplate)
{
	//hide all supportive markers and detect steth marker
	IplImage * imageWithNoMarkers = HideSupportiveMarkers(sourceImage, 
		localCallibrationData->GetCenterMarkerPoint(), centerMarkerTemplate,
		localCallibrationData->GetLeftMarkerPoint(), leftMarkerTemplate,
		localCallibrationData->GetRightMarkerPoint(), rightMarkerTemplate);

	CvPoint stethMarkerMatchPoint = SupportiveFunctions::GetMatchWithMarker(imageWithNoMarkers, stethMarkerTemplate);
	MarkerPoint stethMarkerPoint;
	stethMarkerPoint.SetMarkerPoint(stethMarkerMatchPoint);
	if (SupportiveFunctions::WithinRegion(imageWithNoMarkers, stethMarkerTemplate, stethMarkerMatchPoint))
	{
		IplImage * stethMarkerMatchedImage = SupportiveFunctions::GetCroppedImage(imageWithNoMarkers, 
			cvRect(stethMarkerMatchPoint.x - stethMarkerTemplate->width / 2,
				stethMarkerMatchPoint.y - stethMarkerTemplate->height / 2,
				stethMarkerTemplate->width,
				stethMarkerTemplate->height));
		int difference = SupportiveFunctions::GetDifference(stethMarkerTemplate, stethMarkerMatchedImage, cvPoint(stethMarkerMatchedImage->width / 2, stethMarkerMatchedImage->height / 2));
		int successScore = (int)((1 - ((float)difference / (float)(stethMarkerMatchedImage->width * stethMarkerMatchedImage->height))) * 100);
		stethMarkerPoint.SetSuccessScore(successScore);
		cvReleaseImage(&stethMarkerMatchedImage);
	}
	else
	{
		stethMarkerPoint.SetSuccessScore(0);
	}

	cvReleaseImage(&imageWithNoMarkers);

	return stethMarkerPoint;
}

StethLocationData* GetMapLocationData(CvPoint stethLocalLocation)
{
	float distanceJugularNotchAndXiphoidProcessPoint = MathUtil::GetDistance(localCallibrationData->GetJugularNotchPoint(),
		localCallibrationData->GetXiphoidProcessPoint());

	CvPoint transformedJugularNotch = cvPoint(0, 0);
	CvPoint transformedXiphoidProcessPoint = cvPoint(0, distanceJugularNotchAndXiphoidProcessPoint);
	CvPoint transformedLeftCostalMargin = MathUtil::GetTranslatedPoint(localCallibrationData->GetJugularNotchPoint(),
		localCallibrationData->GetXiphoidProcessPoint(),
		localCallibrationData->GetLeftCostalMarginPoint(),
		transformedJugularNotch,
		transformedXiphoidProcessPoint);
	CvPoint transformedRightCostalMargin = MathUtil::GetTranslatedPoint(localCallibrationData->GetJugularNotchPoint(),
		localCallibrationData->GetXiphoidProcessPoint(),
		localCallibrationData->GetRightCostalMarginPoint(),
		transformedJugularNotch,
		transformedXiphoidProcessPoint);
	CvPoint transformedStethLocation = MathUtil::GetTranslatedPoint(localCallibrationData->GetJugularNotchPoint(),
		localCallibrationData->GetXiphoidProcessPoint(),
		stethLocalLocation,
		transformedJugularNotch,
		transformedXiphoidProcessPoint);

	StethLocationData* locationData = new StethLocationData();
	locationData->JugularNotchPointX = transformedJugularNotch.x;
	locationData->JugularNotchPointY = transformedJugularNotch.y;
	locationData->XiphoidProcessPointX = transformedXiphoidProcessPoint.x;
	locationData->XiphoidProcessPointY = transformedXiphoidProcessPoint.y;
	locationData->LeftCostalMarginPointX = transformedLeftCostalMargin.x;
	locationData->LeftCostalMarginPointY = transformedLeftCostalMargin.y;
	locationData->RightCostalMarginPointX = transformedRightCostalMargin.x;
	locationData->RightCostalMarginPointY = transformedRightCostalMargin.y;
	locationData->StethLocationX = transformedStethLocation.x;
	locationData->StethLocationY = transformedStethLocation.y;
	return locationData;
}